#include <iostream>
#include <fstream>
#include <iomanip>    // For std::setw, std::fixed, std::setprecision, std::hex, std::dec
#include <vector>     // For std::vector
#include <algorithm>  // For std::min, std::max
#include <cmath>      // For fabs, roundf, powf, floorf
#include <limits>     // For std::numeric_limits
#include <string>     // For std::string
#include <cstdint>    // For int32_t, uint16_t, uint8_t

// 包含顶层HLS模块的头文件，HLS模块内部会使用ap_fixed/ap_uint
#include "gpu_render.hpp" // 包含 fpal.hpp, pbirsm.hpp, hr_tbr.hpp, tbr_rasterizer.hpp
#include "bun_zipper_res4_data.h" // 包含斯坦福兔子模型数据

// =========================================================================================
// PS端数据类型定义和转换函数 (模拟 HLS ap_fixed/ap_uint 的行为)
// -----------------------------------------------------------------------------------------
// PS端没有 ap_fixed.h，所以我们需要手动模拟其行为，特别是 bit-level 的转换。
// 这些函数将用于将 PS 端浮点/整数数据转换为 PL 端期望的底层位模式，反之亦然。
// =========================================================================================

// ==================== fp_coord_t (ap_fixed<28, 18, AP_RND_CONV, AP_SAT>) ====================
// HLS定义：typedef ap_fixed<28, 18, AP_RND_CONV, AP_SAT> fp_coord_t;
// W=28, I=18, F=10 (W-I)
const int PS_FP_COORD_W = 28; // Total bits
const int PS_FP_COORD_I = 18; // Integer bits (including sign)
const int PS_FP_COORD_F = PS_FP_COORD_W - PS_FP_COORD_I; // Fractional bits = 10
const float PS_FP_COORD_SCALE_FACTOR = powf(2.0f, PS_FP_COORD_F); // 2^10 = 1024.0f

// HLS ap_fixed<W,I> 内部存储为 W 位的有符号整数。
// 对于 28 位，其范围是 [-2^27, 2^27 - 1]。
const int64_t PS_FP_COORD_MAX_INTERNAL_VAL = (1LL << (PS_FP_COORD_W - 1)) - 1;   // max positive: 134217727
const int64_t PS_FP_COORD_MIN_INTERNAL_VAL = -(1LL << (PS_FP_COORD_W - 1));     // min negative: -134217728

/**
 * @brief 将 PS 端浮点数 (float) 转换为 HLS IP核期望的 28 位定点数内部整数表示 (int32_t)。
 *        模拟 ap_fixed 的舍入 (AP_RND_CONV, round half up) 和饱和 (AP_SAT) 行为。
 *        这个内部整数值会被写入 DDR/AXI-Lite。
 * @param val 输入浮点数
 * @param debug_prefix 调试信息前缀，用于区分不同参数
 * @param is_debug 是否开启调试打印
 * @return 转换为 28 位定点数的内部整数表示 (int32_t)。
 */
int32_t ps_float_to_fp_coord_internal(float val, const std::string& debug_prefix = "", bool is_debug = false) {
    // 1. Scale: 乘以 2^F
    float scaled_val = val * PS_FP_COORD_SCALE_FACTOR;
    
    // 2. Rounding: AP_RND_CONV 是 round half to even (IEEE 754 standard rounding)
    // C++11 std::round 实现了 round half to even
    long long internal_val_before_sat = static_cast<long long>(std::round(scaled_val));

    // 3. Saturation: 确保值在 28 位有符号整数的范围内
    long long internal_val = std::max(PS_FP_COORD_MIN_INTERNAL_VAL, std::min(internal_val_before_sat, PS_FP_COORD_MAX_INTERNAL_VAL));
    
    #ifndef __SYNTHESIS__
    if (is_debug) {
        std::cout << debug_prefix << "Input float: " << std::fixed << std::setprecision(6) << val 
                  << ", Scaled: " << scaled_val 
                  << ", Internal before sat: " << internal_val_before_sat 
                  << ", Internal after sat (int32_t): " << internal_val << std::endl;
    }
    #endif

    return static_cast<int32_t>(internal_val); // 返回 28 位有符号整数
}

/**
 * @brief 将 HLS IP核的 28 位定点数内部整数表示 (int32_t) 转换回 PS 端浮点数 (float)。
 *        用于从 IP 核读取定点数值时进行解释。
 * @param internal_val IP核输出的 28 位定点数内部整数表示
 * @param debug_prefix 调试信息前缀
 * @param is_debug 是否开启调试打印
 * @return 转换后的浮点数
 */
float ps_fp_coord_internal_to_float(int32_t internal_val, const std::string& debug_prefix = "", bool is_debug = false) {
    float converted_float = static_cast<float>(internal_val) / PS_FP_COORD_SCALE_FACTOR;
    #ifndef __SYNTHESIS__
    if (is_debug) {
        std::cout << debug_prefix << "Input internal (int32_t): " << internal_val 
                  << ", Converted float: " << std::fixed << std::setprecision(6) << converted_float << std::endl;
    }
    #endif
    return converted_float;
}


// ==================== fp_z_buffer_t (ap_uint<16>) ====================
// HLS定义：typedef ap_uint<16> fp_z_buffer_t;
// PS端直接使用 uint16_t 即可表示 16 位无符号整数
typedef uint16_t ps_fp_z_buffer_t;

/**
 * @brief 将 PS 端浮点数 (例如归一化深度) 转换为 16 位无符号 Z Buffer 值 (0-65535)。
 *        模拟 HLS ap_uint<16> 的截断行为。
 * @param z_float 输入浮点数
 * @param debug_prefix 调试信息前缀
 * @param is_debug 是否开启调试打印
 * @return 转换后的 16 位无符号 Z Buffer 值 (uint16_t)
 */
uint16_t ps_float_to_fp_z_buffer(float z_float, const std::string& debug_prefix = "", bool is_debug = false) {
    // HLS ap_uint<16>(float_val) 的行为是先取整（默认trunc），然后截断到16位。
    // 为了更接近 HLS 的 ap_uint(float) 行为，我们直接对浮点数进行截断（floor），然后进行钳制。
    float clamped_val = std::max(0.0f, std::min(z_float, 65535.0f)); // 钳制到 [0, 65535] 范围
    uint16_t result = static_cast<uint16_t>(floorf(clamped_val)); // 截断取整
    #ifndef __SYNTHESIS__
    if (is_debug) {
        std::cout << debug_prefix << "Input float: " << std::fixed << std::setprecision(6) << z_float 
                  << ", Clamped: " << clamped_val 
                  << ", Converted uint16_t: " << result << std::endl;
    }
    #endif
    return result;
}

/**
 * @brief 将 16 位无符号 Z Buffer 值转换回浮点数。
 * @param z_uint16 16 位无符号 Z Buffer 值
 * @return 转换后的浮点数
 */
float ps_fp_z_buffer_to_float(uint16_t z_uint16) {
    return static_cast<float>(z_uint16);
}


// ==================== fp_color_channel_t (ap_uint<8>) ====================
// HLS定义：typedef ap_uint<8> fp_color_channel_t;
// PS端直接使用 uint8_t 即可表示 8 位无符号整数
typedef uint8_t ps_fp_color_channel_t;

/**
 * @brief 将 PS 端浮点数或整数转换为 8 位无符号颜色通道值 (0-255)。
 *        模拟 HLS ap_uint<8> 的截断行为。
 * @param color_val 输入数值
 * @param debug_prefix 调试信息前缀
 * @param is_debug 是否开启调试打印
 * @return 转换后的 8 位无符号颜色通道值 (uint8_t)
 */
uint8_t ps_to_fp_color_channel(float color_val, const std::string& debug_prefix = "", bool is_debug = false) {
    // 钳制到 [0, 255] 范围
    float clamped_val = std::max(0.0f, std::min(color_val, 255.0f));
    uint8_t result = static_cast<uint8_t>(floorf(clamped_val)); // 截断取整
    #ifndef __SYNTHESIS__
    if (is_debug) {
        std::cout << debug_prefix << "Input float: " << std::fixed << std::setprecision(6) << color_val 
                  << ", Clamped: " << clamped_val 
                  << ", Converted uint8_t: " << (int)result << std::endl;
    }
    #endif
    return result;
}

/**
 * @brief 将 8 位无符号颜色通道值转换回浮点数。
 * @param color_uint8 8 位无符号颜色通道值
 * @return 转换后的浮点数
 */
float ps_fp_color_channel_to_float(uint8_t color_uint8) {
    return static_cast<float>(color_uint8);
}


// =========================================================================================
// DDR 数据打包/解包函数 (处理 64 位 ddr_word_t)
// -----------------------------------------------------------------------------------------
// 这些函数根据 HLS 模块中定义的数据布局，将 PS 端数据打包成 64 位 DDR 字，或从 64 位字中解包数据。
// =========================================================================================

// 从 PBIRSM.hpp 获取的 DDR AXI Master 字宽定义
typedef ap_uint<AXI_STREAM_WORD_BITS> ddr_word_t; // 64位

/**
 * @brief 将 R, G, B 分量打包成 32 位颜色槽 (低 24 位是 BGR)。
 *        用于 PS 端初始化 Frame Buffer 或验证读取。
 *        与 HLS 端打包逻辑保持一致：B (bits 0-7), G (bits 8-15), R (bits 16-23)。
 * @param r, g, b 颜色分量 (0-255)
 * @return 打包后的 32 位颜色槽值 (uint32_t)，高 8 位为 0。
 */
uint32_t ps_pack_rgb_to_32bit_slot(uint8_t r, uint8_t g, uint8_t b) {
    uint32_t packed_color = 0;
    packed_color |= static_cast<uint32_t>(b) << 0;  // B (0-7)
    packed_color |= static_cast<uint32_t>(g) << 8;  // G (8-15)
    packed_color |= static_cast<uint32_t>(r) << 16; // R (16-23)
    return packed_color; // 高 8 位是 0
}

/**
 * @brief 从 32 位颜色槽值中解包 R, G, B 分量。
 *        用于 PS 端从 Frame Buffer 读取后解析颜色。
 *        与 HLS 端打包逻辑保持一致：B (bits 0-7), G (bits 8-15), R (bits 16-23)。
 * @param slot_val 32 位颜色槽值
 * @param r, g, b 输出的 R, G, B 分量
 */
void ps_unpack_32bit_slot_to_rgb(uint32_t slot_val, uint8_t& r, uint8_t& g, uint8_t& b) {
    b = static_cast<uint8_t>((slot_val >> 0) & 0xFF);
    g = static_cast<uint8_t>((slot_val >> 8) & 0xFF);
    r = static_cast<uint8_t>((slot_val >> 16) & 0xFF);
}


// =========================================================================================
// 其他辅助定义和函数
// =========================================================================================

// 定义屏幕分辨率 (与 HLS 定义保持一致)
#define TEST_SCREEN_WIDTH 320
#define TEST_SCREEN_HEIGHT 240
#define TOTAL_PIXELS (TEST_SCREEN_WIDTH * TEST_SCREEN_HEIGHT)

// RGB 结构体，用于帧缓冲区
struct RGB {
    unsigned char b, g, r;
};

// --- 简单的 3D 向量和矩阵数学库 ---
struct Vec3 {
    float x, y, z;
    Vec3 operator-(const Vec3& other) const { return {x - other.x, y - other.y, z - other.z}; }
    Vec3 cross(const Vec3& other) const {
        return {y * other.z - z * other.y, z * other.x - x * other.z, x * other.y - y * other.x};
    }
    float dot(const Vec3& other) const { return x * other.x + y * other.y + z * other.z; }
    float length() const { return std::sqrt(x * x + y * y + z * z); }
    Vec3 normalize() const {
        float len = length();
        return (len > 1e-6f) ? Vec3{x / len, y / len, z / len} : Vec3{0, 0, 0};
    }
    Vec3 operator*(float s) const { return {x * s, y * s, z * s}; }
    Vec3 operator+(const Vec3& other) const { return {x + other.x, y + other.y, z + other.z}; }
};

struct Vec4 {
    float x, y, z, w;
};

struct Mat4 {
    float m[4][4];

    static Mat4 identity() {
        Mat4 res;
        for (int i = 0; i < 4; ++i)
            for (int j = 0; j < 4; ++j)
                res.m[i][j] = (i == j) ? 1.0f : 0.0f;
        return res;
    }

    Mat4 operator*(const Mat4& other) const {
        Mat4 result;
        for (int i = 0; i < 4; ++i) {
            for (int j = 0; j < 4; ++j) {
                result.m[i][j] = 0.0f;
                for (int k = 0; k < 4; ++k) {
                    result.m[i][j] += m[i][k] * other.m[k][j];
                }
            }
        }
        return result;
    }

    Vec4 operator*(const Vec4& v) const {
        Vec4 result;
        result.x = m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w;
        result.y = m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w;
        result.z = m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w;
        result.w = m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w;
        return result;
    }
};

Mat4 translate(const Mat4& mat, const Vec3& v) {
    Mat4 result = mat;
    result.m[0][3] += v.x;
    result.m[1][3] += v.y;
    result.m[2][3] += v.z;
    return result;
}

Mat4 scale(const Mat4& mat, const Vec3& v) {
    Mat4 result = mat;
    result.m[0][0] *= v.x;
    result.m[1][1] *= v.y;
    result.m[2][2] *= v.z;
    return result;
}

Mat4 rotateY(const Mat4& mat, float angle) {
    float c = std::cos(angle);
    float s = std::sin(angle);
    Mat4 rot = Mat4::identity();
    rot.m[0][0] = c;
    rot.m[0][2] = s;
    rot.m[2][0] = -s;
    rot.m[2][2] = c;
    return mat * rot;
}

Mat4 lookAt(const Vec3& eye, const Vec3& center, const Vec3& up) {
    Vec3 f = (center - eye).normalize();
    Vec3 u = up.normalize();
    Vec3 s = f.cross(u).normalize();
    u = s.cross(f); // Recalculate u to be truly orthogonal to s and f

    Mat4 result = Mat4::identity();
    result.m[0][0] = s.x;
    result.m[0][1] = s.y;
    result.m[0][2] = s.z;
    result.m[1][0] = u.x;
    result.m[1][1] = u.y;
    result.m[1][2] = u.z;
    result.m[2][0] = -f.x;
    result.m[2][1] = -f.y;
    result.m[2][2] = -f.z;
    result.m[0][3] = -s.dot(eye);
    result.m[1][3] = -u.dot(eye);
    result.m[2][3] = f.dot(eye);
    return result;
}

Mat4 perspective(float fovY, float aspect, float zNear, float zFar) {
    Mat4 result = Mat4::identity();
    float tanHalfFovY = std::tan(fovY / 2.0f);
    result.m[0][0] = 1.0f / (aspect * tanHalfFovY);
    result.m[1][1] = 1.0f / tanHalfFovY;
    result.m[2][2] = -(zFar + zNear) / (zFar - zNear);
    result.m[2][3] = -(2.0f * zFar * zNear) / (zFar - zNear);
    result.m[3][2] = -1.0f;
    result.m[3][3] = 0.0f;
    return result;
}
// --- 3D 数学库结束 ---

// 辅助函数：计算三角形的边缘方程系数 (Ax + By + C = 0)
void calculate_edge_equations(
    float v0x, float v0y, float v1x, float v1y, float v2x, float v2y,
    float& A0_f, float& B0_f, float& C0_f,
    float& A1_f, float& B1_f, float& C1_f,
    float& A2_f, float& B2_f, float& C2_f
) {
    // Edge 0: v0 -> v1
    A0_f = v0y - v1y;
    B0_f = v1x - v0x;
    C0_f = v0x * v1y - v1x * v0y;
    
    // Edge 1: v1 -> v2
    A1_f = v1y - v2y;
    B1_f = v2x - v1x;
    C1_f = v1x * v2y - v2x * v1y;

    // Edge 2: v2 -> v0
    A2_f = v2y - v0y;
    B2_f = v0x - v2x;
    C2_f = v2x * v0y - v0x * v2y;
}

// 辅助函数：计算深度梯度 (dzdx, dzdy)
void calculate_depth_gradients(
    float v0x, float v0y, float v0z, // v0z is screen-space Z (0-65535)
    float v1x, float v1y, float v1z,
    float v2x, float v2y, float v2z,
    float& dzdx_f, float& dzdy_f
) {
    float dx1 = v1x - v0x;
    float dy1 = v1y - v0y;
    float dz1 = v1z - v0z; 

    float dx2 = v2x - v0x;
    float dy2 = v2y - v0y;
    float dz2 = v2z - v0z; 

    float det = dx1 * dy2 - dx2 * dy1;

    if (fabs(det) < 1e-6) { // 防止除零
        dzdx_f = 0.0f;
        dzdy_f = 0.0f;
        return;
    }

    float inv_det = 1.0f / det;
    dzdx_f = (dz1 * dy2 - dz2 * dy1) * inv_det;
    dzdy_f = (dx1 * dz2 - dx2 * dy1) * inv_det;
}

// BMP 文件头结构 (14字节)
#pragma pack(push, 1) 
struct BMPFileHeader {
    unsigned short bfType;      
    unsigned int   bfSize;      
    unsigned short bfReserved1; 
    unsigned short bfReserved2; 
    unsigned int   bfOffBits;   
};

// BMP 信息头结构 (40字节)
struct BMPInfoHeader {
    unsigned int   biSize;          
    int            biWidth;         
    int            biHeight;        
    unsigned short biPlanes;        
    unsigned short biBitCount;      
    unsigned int   biCompression;   
    unsigned int   biSizeImage;     
    int            biXPelsPerMeter; 
    unsigned int   biYPelsPerMeter; 
    unsigned int   biClrUsed;       
    unsigned int   biClrImportant;  
};
#pragma pack(pop)

// 辅助函数：将帧缓冲区数据写入BMP文件
void write_bmp(const char* filename, int width, int height, const std::vector<RGB>& framebuffer) {
    std::ofstream file(filename, std::ios::binary);
    if (!file) {
        std::cerr << "Error: Could not open file " << filename << " for writing." << std::endl;
        return;
    }

    int row_padded_size = (width * 3 + 3) & (~3); 
    int image_size = row_padded_size * height;

    BMPFileHeader file_header;
    file_header.bfType = 0x4D42; 
    file_header.bfSize = sizeof(BMPFileHeader) + sizeof(BMPInfoHeader) + image_size;
    file_header.bfReserved1 = 0;
    file_header.bfReserved2 = 0;
    file_header.bfOffBits = sizeof(BMPFileHeader) + sizeof(BMPInfoHeader);

    BMPInfoHeader info_header;
    info_header.biSize = sizeof(BMPInfoHeader);
    info_header.biWidth = width;
    info_header.biHeight = height; 
    info_header.biPlanes = 1;
    info_header.biBitCount = 24; 
    info_header.biCompression = 0; 
    info_header.biSizeImage = image_size;
    info_header.biXPelsPerMeter = 2835; 
    info_header.biYPelsPerMeter = 2835; 
    info_header.biClrUsed = 0;
    info_header.biClrImportant = 0;

    file.write(reinterpret_cast<char*>(&file_header), sizeof(file_header));
    file.write(reinterpret_cast<char*>(&info_header), sizeof(info_header));
    
    std::vector<unsigned char> row_buffer(row_padded_size);
    for (int y = height - 1; y >= 0; --y) { 
        for (int x = 0; x < width; ++x) {
            row_buffer[x * 3 + 0] = framebuffer[y * width + x].b;
            row_buffer[x * 3 + 1] = framebuffer[y * width + x].g;
            row_buffer[x * 3 + 2] = framebuffer[y * width + x].r;
        }
        file.write(reinterpret_cast<char*>(row_buffer.data()), row_padded_size);
    }

    file.close();
    std::cout << "Successfully wrote " << filename << std::endl;
}


int main() {
    std::cout << "--- GPU_Render Top-Level Module Testbench ---" << std::endl;

    // --- SELF-CHECK: Color Pack/Unpack Logic ---
    std::cout << "\n[TESTBENCH] Self-checking Color Pack/Unpack Logic..." << std::endl;
    uint8_t test_r = 0xAA, test_g = 0xBB, test_b = 0xCC; // Example: R=AA, G=BB, B=CC
    uint32_t packed_slot = ps_pack_rgb_to_32bit_slot(test_r, test_g, test_b);
    uint8_t unpacked_r, unpacked_g, unpacked_b;
    ps_unpack_32bit_slot_to_rgb(packed_slot, unpacked_r, unpacked_g, unpacked_b);

    std::cout << "  Original (R,G,B): (" << std::hex << (int)test_r << "," << (int)test_g << "," << (int)test_b << std::dec << ")" << std::endl;
    std::cout << "  Packed Slot [hex]: " << std::hex << std::setw(8) << std::setfill('0') << packed_slot << std::dec << std::endl;
    std::cout << "  Unpacked (R,G,B): (" << std::hex << (int)unpacked_r << "," << (int)unpacked_g << "," << (int)unpacked_b << std::dec << ")" << std::endl;

    if (test_r == unpacked_r && test_g == unpacked_g && test_b == unpacked_b) {
        std::cout << "  Color pack/unpack self-check PASSED." << std::endl;
    } else {
        std::cerr << "  ERROR: Color pack/unpack self-check FAILED! Mismatch detected." << std::endl;
    }
    std::cout << "-----------------------------------------------------" << std::endl;
    // --- END SELF-CHECK ---

    // --- 1. 模拟 DDR 缓冲区 (使用 std::vector 作为内存块) ---
    const uint32_t ZBUFFER_ALLOC_WORDS = TEST_SCREEN_WIDTH * TEST_SCREEN_HEIGHT / Z_PER_WORD; 
    const uint32_t FRAMEBUFFER_ALLOC_WORDS = TEST_SCREEN_WIDTH * TEST_SCREEN_HEIGHT / COLOR_PER_WORD;
    // PARAMS_ALLOC_WORDS 应该足够大，以容纳所有原始的兔子三角形数据
    const uint32_t PARAMS_ALLOC_WORDS = BUNNY_NUM_TRIANGLES * NUM_AXI_WORDS_PER_PACKET; 
    
    std::vector<ddr_word_t> simulated_zbuffer_ddr_storage(ZBUFFER_ALLOC_WORDS);
    std::vector<ddr_word_t> simulated_framebuffer_ddr(FRAMEBUFFER_ALLOC_WORDS);
    std::vector<ddr_word_t> simulated_params_ddr(PARAMS_ALLOC_WORDS); // 存储所有三角形的参数

    // 初始化Z buffer为最远深度 (65535)
    for (uint32_t i = 0; i < ZBUFFER_ALLOC_WORDS; ++i) { 
        simulated_zbuffer_ddr_storage[i] = 0xFFFF'FFFF'FFFF'FFFFULL; // 填充所有 16 位 Z 值为 65535
    }
    std::cout << "[TESTBENCH] Initialized Z-buffer with all 65535 (farthest)." << std::endl;

    // 初始化 Frame buffer 为默认背景色 (灰色 BGR: 0x323232)
    uint32_t default_color_slot = ps_pack_rgb_to_32bit_slot(0x32, 0x32, 0x32); // R=50, G=50, B=50
    for (uint32_t i = 0; i < FRAMEBUFFER_ALLOC_WORDS; ++i) { 
        simulated_framebuffer_ddr[i] = ((ddr_word_t)default_color_slot << 32) | (ddr_word_t)default_color_slot;
    }
    std::cout << "[TESTBENCH] Initialized Frame-buffer with default grey (BGR: 0x323232)." << std::endl;


    // --- 2. 准备斯坦福兔子数据并进行 MVP 变换 ---
    std::cout << "\n[TESTBENCH] Preparing Stanford Bunny data with MVP transformation..." << std::endl;

    struct PSTriangle {
        float v0x, v0y, v0z; 
        float v1x, v1y, v1z;
        float v2x, v2y, v2z;
        uint8_t r, g, b; // 纯色
        float edge0_A, edge0_B, edge0_C;
        float edge1_A, edge1_B, edge1_C;
        float edge2_A, edge2_B, edge2_C;
        float dzdx, dzdy;
    };

    std::vector<PSTriangle> all_processed_triangles_ps; // 存储所有处理后的三角形
    all_processed_triangles_ps.reserve(BUNNY_NUM_TRIANGLES);

    // --- MVP 矩阵设置 ---
    // Model Matrix (M): 缩放、旋转、平移兔子
    Mat4 model = Mat4::identity();
    model = translate(model, {0.0f, -0.4f, 0.0f}); // 将兔子向下移动一点，使其底座在y=0附近
    model = rotateY(model, static_cast<float>(M_PI / 4.0)); // 绕Y轴旋转45度
    model = scale(model, {3.0f, 3.0f, 3.0f}); // 放大兔子，原始坐标很小

    // View Matrix (V): 摄像机设置
    Vec3 cameraPos    = {0.0f, 0.5f, 4.0f};  // 摄像机位置
    Vec3 cameraTarget = {0.0f, 0.0f, 0.0f};  // 摄像机看向的目标
    Vec3 cameraUp     = {0.0f, 1.0f, 0.0f};  // 摄像机上向量
    Mat4 view = lookAt(cameraPos, cameraTarget, cameraUp);

    // Projection Matrix (P): 透视投影
    float fovY = static_cast<float>(M_PI / 3.0f); // 60度视场角
    float aspect = (float)TEST_SCREEN_WIDTH / (float)TEST_SCREEN_HEIGHT; // 宽高比
    float zNear = 0.1f; // 近裁剪面
    float zFar = 100.0f; // 远裁剪面
    Mat4 projection = perspective(fovY, aspect, zNear, zFar);

    Mat4 mvp = projection * view * model;

    // --- 淡黄色设置 ---
    uint8_t bunny_r = 255;
    uint8_t bunny_g = 255;
    uint8_t bunny_b = 150; // 淡黄色

    // 遍历所有三角形
    for (int i = 0; i < BUNNY_NUM_TRIANGLES; ++i) {
        // 获取顶点索引
        unsigned int idx0 = bunny_indices[i][0];
        unsigned int idx1 = bunny_indices[i][1];
        unsigned int idx2 = bunny_indices[i][2];

        // 获取模型空间顶点坐标
        Vec4 v0_model = {bunny_vertices[idx0][0], bunny_vertices[idx0][1], bunny_vertices[idx0][2], 1.0f};
        Vec4 v1_model = {bunny_vertices[idx1][0], bunny_vertices[idx1][1], bunny_vertices[idx1][2], 1.0f};
        Vec4 v2_model = {bunny_vertices[idx2][0], bunny_vertices[idx2][1], bunny_vertices[idx2][2], 1.0f};

        // MVP 变换
        Vec4 v0_clip = mvp * v0_model;
        Vec4 v1_clip = mvp * v1_model;
        Vec4 v2_clip = mvp * v2_model;
        
        // --- 裁剪 (简单 W 裁剪) ---
        // 任何顶点的 w < 0 或裁剪空间坐标超出 [-w, w] 都可能导致被裁剪
        // 这里的简单裁剪只是为了避免除零和渲染明显在视图外的三角形
        // 实际管线中会进行更复杂的裁剪算法
        if (v0_clip.w <= 0.001f || v1_clip.w <= 0.001f || v2_clip.w <= 0.001f) continue;
        
        // 透视除法得到 NDC 坐标
        Vec4 v0_ndc = {v0_clip.x / v0_clip.w, v0_clip.y / v0_clip.w, v0_clip.z / v0_clip.w, 1.0f};
        Vec4 v1_ndc = {v1_clip.x / v1_clip.w, v1_clip.y / v1_clip.w, v1_clip.z / v1_clip.w, 1.0f};
        Vec4 v2_ndc = {v2_clip.x / v2_clip.w, v2_clip.y / v2_clip.w, v2_clip.z / v2_clip.w, 1.0f};

        // 再次进行粗略的裁剪，检查 NDC 范围
        auto is_outside_ndc = [](const Vec4& v) {
            return v.x < -1.0f || v.x > 1.0f || v.y < -1.0f || v.y > 1.0f || v.z < -1.0f || v.z > 1.0f;
        };
        // 如果所有顶点都在裁剪空间外，则跳过
        if (is_outside_ndc(v0_ndc) && is_outside_ndc(v1_ndc) && is_outside_ndc(v2_ndc)) continue;


        // 视口变换到屏幕坐标
        // X_screen = (X_ndc + 1) * 0.5 * ScreenWidth
        // Y_screen = (1 - Y_ndc) * 0.5 * ScreenHeight (Y轴翻转，因为屏幕坐标系Y轴向下)
        // Z_screen = (Z_ndc + 1) * 0.5 * 65535 (HLS ZBuffer是无符号的 0-65535)
        float v0x_screen = (v0_ndc.x + 1.0f) * 0.5f * TEST_SCREEN_WIDTH;
        float v0y_screen = (1.0f - v0_ndc.y) * 0.5f * TEST_SCREEN_HEIGHT;
        float v0z_screen = (v0_ndc.z + 1.0f) * 0.5f * 65535.0f; // Scale to Z buffer max

        float v1x_screen = (v1_ndc.x + 1.0f) * 0.5f * TEST_SCREEN_WIDTH;
        float v1y_screen = (1.0f - v1_ndc.y) * 0.5f * TEST_SCREEN_HEIGHT;
        float v1z_screen = (v1_ndc.z + 1.0f) * 0.5f * 65535.0f;

        float v2x_screen = (v2_ndc.x + 1.0f) * 0.5f * TEST_SCREEN_WIDTH;
        float v2y_screen = (1.0f - v2_ndc.y) * 0.5f * TEST_SCREEN_HEIGHT;
        float v2z_screen = (v2_ndc.z + 1.0f) * 0.5f * 65535.0f;
        
        // 构造 PSTriangle
        PSTriangle tri;
        tri.v0x = v0x_screen; tri.v0y = v0y_screen; tri.v0z = v0z_screen;
        tri.v1x = v1x_screen; tri.v1y = v1y_screen; tri.v1z = v1z_screen;
        tri.v2x = v2x_screen; tri.v2y = v2y_screen; tri.v2z = v2z_screen;
        tri.r = bunny_r; tri.g = bunny_g; tri.b = bunny_b;

        // 计算边缘方程和深度梯度
        calculate_edge_equations(tri.v0x, tri.v0y, tri.v1x, tri.v1y, tri.v2x, tri.v2y,
                                 tri.edge0_A, tri.edge0_B, tri.edge0_C,
                                 tri.edge1_A, tri.edge1_B, tri.edge1_C,
                                 tri.edge2_A, tri.edge2_B, tri.edge2_C);
        calculate_depth_gradients(tri.v0x, tri.v0y, tri.v0z,
                                  tri.v1x, tri.v1y, tri.v1z,
                                  tri.v2x, tri.v2y, tri.v2z,
                                  tri.dzdx, tri.dzdy);
        
        all_processed_triangles_ps.push_back(tri);
    }
    
    const uint32_t total_valid_triangles = all_processed_triangles_ps.size();
    std::cout << "[TESTBENCH] Processed " << total_valid_triangles << " valid triangles for the bunny." << std::endl;


    // --- 3. 准备 PBIRSM 的模拟 DDR 输入 (PS端数据转换为 PL 端位模式并写入模拟DDR) ---
    // 一次性将所有三角形参数写入模拟的DDR缓冲区
    std::cout << "\n[TESTBENCH] Writing ALL triangle parameters to simulated DDR (PS-to-PL data conversion)..." << std::endl;
    for (uint32_t i = 0; i < total_valid_triangles; ++i) { 
        int current_packet_word_offset = i * NUM_AXI_WORDS_PER_PACKET;
        const PSTriangle& tri = all_processed_triangles_ps[i];
        
        int word_idx = 0; // 当前数据包内的字索引
        // bool debug_this_triangle = (i == 0); // 只详细打印第一个三角形
        bool debug_this_triangle = false; // 关闭详细调试打印，因为三角形数量很多

        // v0_x, v0_y, v1_x, v1_y, v2_x, v2_y (6 * fp_coord_t = 6 words)
        // 写入 fp_coord_t 的内部 int32_t 表示
        int32_t internal_val;
        ddr_word_t ddr_word;

        internal_val = ps_float_to_fp_coord_internal(tri.v0x, "  v0_x: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val)); 
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        internal_val = ps_float_to_fp_coord_internal(tri.v0y, "  v0_y: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        
        internal_val = ps_float_to_fp_coord_internal(tri.v1x, "  v1_x: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        internal_val = ps_float_to_fp_coord_internal(tri.v1y, "  v1_y: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        internal_val = ps_float_to_fp_coord_internal(tri.v2x, "  v2_x: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        internal_val = ps_float_to_fp_coord_internal(tri.v2y, "  v2_y: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        // v0_z, v1_z, v2_z (3 * fp_z_buffer_t = 3 words)
        // 将 16 位值放入 64 位字的低 16 位
        uint16_t z_val;
        z_val = ps_float_to_fp_z_buffer(tri.v0z, "  v0_z: ", debug_this_triangle); 
        ddr_word = static_cast<ddr_word_t>(z_val); 
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        z_val = ps_float_to_fp_z_buffer(tri.v1z, "  v1_z: ", debug_this_triangle); 
        ddr_word = static_cast<ddr_word_t>(z_val);
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        z_val = ps_float_to_fp_z_buffer(tri.v2z, "  v2_z: ", debug_this_triangle); 
        ddr_word = static_cast<ddr_word_t>(z_val);
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        // Edge equations (fp_coord_t)
        internal_val = ps_float_to_fp_coord_internal(tri.edge0_A, "  e0_A: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        internal_val = ps_float_to_fp_coord_internal(tri.edge0_B, "  e0_B: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        internal_val = ps_float_to_fp_coord_internal(tri.edge0_C, "  e0_C: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        internal_val = ps_float_to_fp_coord_internal(tri.edge1_A, "  e1_A: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        internal_val = ps_float_to_fp_coord_internal(tri.edge1_B, "  e1_B: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        internal_val = ps_float_to_fp_coord_internal(tri.edge1_C, "  e1_C: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        internal_val = ps_float_to_fp_coord_internal(tri.edge2_A, "  e2_A: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        internal_val = ps_float_to_fp_coord_internal(tri.edge2_B, "  e2_B: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        internal_val = ps_float_to_fp_coord_internal(tri.edge2_C, "  e2_C: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        // Depth gradients (fp_coord_t)
        internal_val = ps_float_to_fp_coord_internal(tri.dzdx, "  dzdx: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        internal_val = ps_float_to_fp_coord_internal(tri.dzdy, "  dzdy: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(static_cast<uint64_t>(internal_val));
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        // pure_color_R, G, B (fp_color_channel_t) 写入 64 位 DDR 字的低 8 位
        uint8_t color_val;

        color_val = ps_to_fp_color_channel(tri.r, "  Color_R: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(color_val);
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        color_val = ps_to_fp_color_channel(tri.g, "  Color_G: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(color_val);
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;

        color_val = ps_to_fp_color_channel(tri.b, "  Color_B: ", debug_this_triangle);
        ddr_word = static_cast<ddr_word_t>(color_val);
        simulated_params_ddr[current_packet_word_offset + word_idx++] = ddr_word;
        
        if (word_idx != NUM_AXI_WORDS_PER_PACKET) {
            std::cerr << "ERROR: Mismatch in expected words per packet! Expected " << NUM_AXI_WORDS_PER_PACKET << ", but wrote " << word_idx << " for triangle " << i << std::endl;
            return 1; 
        }
    }
    std::cout << "[TESTBENCH] Finished writing " << total_valid_triangles << " triangles into simulated DDR." << std::endl;

    // --- 4. 调用顶层 GPU_Render 模块 (HLS仿真器会自动处理DDR指针的访问) ---
    // HLS IP 核一次最多处理 MAX_NUM_TRIANGLES 个三角形
    const uint16_t triangles_per_batch = MAX_NUM_TRIANGLES; 
    uint32_t current_triangle_offset = 0;
    ap_uint<32> total_write_ops_overall = 0;

    std::cout << "\n[TESTBENCH] Calling GPU_Render_TBR top-level module in batches..." << std::endl;

    while (current_triangle_offset < total_valid_triangles) {
        uint16_t num_triangles_in_batch = std::min(
            (uint32_t)triangles_per_batch, 
            total_valid_triangles - current_triangle_offset
        );

        // 计算当前批次三角形参数在模拟 DDR 中的起始地址
        ddr_word_t* batch_params_base_addr = simulated_params_ddr.data() + (current_triangle_offset * NUM_AXI_WORDS_PER_PACKET);
        
        ap_uint<32> total_write_ops_current_batch = 0; // 每个批次的写入计数

        std::cout << "\n[TESTBENCH] Rendering batch: starting from triangle " 
                  << current_triangle_offset << ", count: " << num_triangles_in_batch << std::endl;

        GPU_Render_TBR(
            batch_params_base_addr,                // 当前批次的参数基地址
            num_triangles_in_batch,                // 当前批次的三角形数量
            TEST_SCREEN_WIDTH,                     // max_x_res (PS to PL)
            TEST_SCREEN_HEIGHT,                    // max_y_res (PS to PL)
            simulated_zbuffer_ddr_storage.data(),  // zbuffer_ddr_raw_ptr (PL to PS)
            simulated_framebuffer_ddr.data(),      // framebuffer_ddr_raw_ptr (PL to PS)
            total_write_ops_current_batch          // total_write_ops_processed (PL to PS)
        );
        
        total_write_ops_overall += total_write_ops_current_batch;
        current_triangle_offset += num_triangles_in_batch;
        std::cout << "[TESTBENCH] Batch finished. Actual writes for this batch: " << total_write_ops_current_batch << std::endl;
    }

    std::cout << "[TESTBENCH] GPU_Render_TBR top-level module (all batches) finished." << std::endl;

    std::cout << "\n------------------------------------" << std::endl;
    std::cout << "Overall Rasterization Result:" << std::endl;
    std::cout << "GPU_Render_TBR reported TOTAL actual DDR writes (across all batches): " << total_write_ops_overall << std::endl;
    std::cout << "------------------------------------" << std::endl;

    // --- 5. 将模拟DDR中的帧缓冲区数据转换为RGB格式并写入BMP文件 (PL到PS数据解析) ---
    std::vector<RGB> final_framebuffer(TOTAL_PIXELS);

    std::cout << "\n[TESTBENCH] PS-Side Reading Framebuffer from simulated DDR & BMP Conversion Debug:" << std::endl;

    for (int i = 0; i < TOTAL_PIXELS; ++i) {
        // 计算当前像素在 64 位 DDR 字中的位置
        uint32_t color_word_idx = i / COLOR_PER_WORD; 
        uint16_t color_sub_word_offset = i % COLOR_PER_WORD; 

        if (color_word_idx >= FRAMEBUFFER_ALLOC_WORDS) { 
            final_framebuffer[i] = {0, 0, 0}; // 越界像素设为黑色
            continue;
        }

        ddr_word_t full_color_word = simulated_framebuffer_ddr[color_word_idx];
        uint32_t packed_pixel_slot; // 32 位颜色槽

        // 根据子字偏移读取对应的 32 位槽
        if (color_sub_word_offset == 0) { // 64位字的低 32 位槽
            packed_pixel_slot = static_cast<uint32_t>(full_color_word & 0xFFFFFFFFULL);
        } else { // 64位字的高 32 位槽
            packed_pixel_slot = static_cast<uint32_t>((full_color_word >> 32) & 0xFFFFFFFFULL);
        }
        
        // 从 32 位槽中解包 24 位 BGR 分量
        uint8_t r, g, b;
        ps_unpack_32bit_slot_to_rgb(packed_pixel_slot, r, g, b);

        final_framebuffer[i] = {b, g, r}; // BMP是BGR格式
    }

    write_bmp("raster_output_combined.bmp", TEST_SCREEN_WIDTH, TEST_SCREEN_HEIGHT, final_framebuffer);

    return 0; // 成功返回 0
}
